import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
from scipy.optimize import minimize
from scipy.integrate import odeint

# 1. 读取并预处理数据
def load_data(file_path):
    data = pd.read_csv(file_path)
    # 转换时间单位为秒
    if data['time'].max() < 10:
        data['time'] = data['time'] * 60
    return data

# 2. 系统辨识
def system_identification(data):
    t = data['time'].values
    y = data['temperature'].values
    u = data['volte'].values
    y0 = y[0]
    u_step = u[0]
    
    # 确定延迟时间τ
    dy_threshold = 0.1  # 温度变化阈值
    dy = y - y0
    idx_start = np.where(dy > dy_threshold)[0]
    if len(idx_start) > 0:
        τ = t[idx_start[0]]
    else:
        τ = 0
    
    # 计算稳态增益K（使用最终温度）
    y_ss = y[-1]
    Δy = y_ss - y0
    K = Δy / u_step
    
    # 两点法估计时间常数T
    t1 = τ + (t[-1] - τ) / 3
    t2 = τ + 2 * (t[-1] - τ) / 3
    
    # 插值获取对应温度
    y1 = np.interp(t1, t, y)
    y2 = np.interp(t2, t, y)
    
    # 计算时间常数T
    Δy1 = y1 - y0
    Δy2 = y2 - y0
    ratio = Δy1 / (K * u_step)
    if ratio >= 1:
        ratio = 0.99
    
    T1 = -(t1 - τ) / np.log(1 - ratio)
    
    ratio = Δy2 / (K * u_step)
    if ratio >= 1:
        ratio = 0.99
    
    T2 = -(t2 - τ) / np.log(1 - ratio)
    T = (T1 + T2) / 2
    
    return K, T, τ

# 3. 系统模型
def first_order_system(y, t, u_func, K, T, τ, dt):
    # 处理时延
    if t < τ:
        u_val = 0
    else:
        u_val = u_func(t - τ)
    dydt = (K * u_val - y) / T
    return dydt

# 4. PID控制
class PIDController:
    def __init__(self, Kp, Ki, Kd, setpoint, output_limits=(0, 5)):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.setpoint = setpoint
        self.output_limits = output_limits
        self.reset()
    
    def reset(self):
        self.integral = 0
        self.prev_error = 0
        self.prev_time = None
    
    def compute(self, y, t):
        error = self.setpoint - y
        
        if self.prev_time is None:
            dt = 0
        else:
            dt = t - self.prev_time
        
        self.integral += error * dt
        
        if dt > 0:
            derivative = (error - self.prev_error) / dt
        else:
            derivative = 0
        
        # PID输出
        output = (self.Kp * error + 
                 self.Ki * self.integral + 
                 self.Kd * derivative)
        
        # 输出限幅
        output = np.clip(output, *self.output_limits)
        
        # 保存状态
        self.prev_error = error
        self.prev_time = t
        
        return output

# 5. 闭环仿真
def closed_loop_simulation(params, K, T, τ, y0, t_sim, dt=1):
    Kp, Ki, Kd = params
    
    # 创建PID控制器
    pid = PIDController(Kp, Ki, Kd, setpoint=35)
    
    # 初始化
    t_points = np.arange(0, t_sim, dt)
    y_points = np.zeros_like(t_points)
    u_points = np.zeros_like(t_points)
    
    y = y0
    pid.reset()
    
    # 控制输入函数
    def u_func(t):
        idx = int(t / dt)
        if idx < 0 or idx >= len(u_points):
            return 0
        return u_points[idx]
    
    # 仿真循环
    for i, t in enumerate(t_points):
        y_points[i] = y      
        u = pid.compute(y, t)
        u_points[i] = u
        if i < len(t_points) - 1:
            t_next = [t, t + dt]
            y_next = odeint(first_order_system, y, t_next, 
                           args=(u_func, K, T, τ, dt))
            y = y_next[-1][0]
    
    return t_points, y_points, u_points

# 6. 性能指标计算
def calculate_performance(t, y, setpoint):
    t10 = None
    t90 = None
    rise_start = None
    rise_end = None
    
    for i, val in enumerate(y):
        if val > setpoint * 0.1 and t10 is None:
            t10 = t[i]
            rise_start = i
        if val > setpoint * 0.9 and t90 is None:
            t90 = t[i]
            rise_end = i
    
    # 上升时间
    rise_time = t90 - t10 if (t90 and t10) else np.nan
    
    # 超调量
    max_y = np.max(y)
    overshoot = (max_y - setpoint) / setpoint * 100 if max_y > setpoint else 0
    
    # 调节时间
    settling_idx = None
    for i in range(len(y)-1, -1, -1):
        if abs(y[i] - setpoint) > setpoint * 0.02:
            settling_idx = i
            break
    
    settling_time = t[settling_idx] if settling_idx else t[-1]
    
    error = setpoint - y
    itae = np.trapz(t * np.abs(error), t)
    
    steady_start_idx = int(len(y) * 0.9)
    steady_state_error = np.mean(error[steady_start_idx:])
    
    return {
        'rise_time': rise_time,
        'overshoot': overshoot,
        'settling_time': settling_time,
        'ITAE': itae,
        'steady_state_error': steady_state_error
    }

# 7. 优化目标函数
def objective_function(params, K, T, τ, y0, t_sim):
    try:
        # 运行仿真
        t, y, _ = closed_loop_simulation(params, K, T, τ, y0, t_sim)
        
        # 计算ITAE性能指标
        setpoint = 35
        error = setpoint - y
        itae = np.trapz(t * np.abs(error), t)
        
        # 添加约束惩罚项
        penalty = 0
        if params[0] < 0:  
            penalty += 1000
        if params[1] < 0:  
            penalty += 1000
        if params[2] < 0:  
            penalty += 1000
        
        return itae + penalty
    except:
        return 1e10  

# 8. 粒子群优化
def pso_optimize(K, T, τ, y0, t_sim, bounds, n_particles=20, max_iter=50):
    # 初始化粒子群
    dim = 3  # Kp, Ki, Kd
    particles = np.random.uniform(low=[b[0] for b in bounds], 
                                 high=[b[1] for b in bounds], 
                                 size=(n_particles, dim))
    velocities = np.zeros((n_particles, dim))
    personal_best = particles.copy()
    personal_best_scores = np.full(n_particles, np.inf)
    global_best = None
    global_best_score = np.inf
    
    # PSO参数因子
    w = 0.7  
    c1 = 1.5  
    c2 = 1.5  
    
    # 优化循环
    for _ in range(max_iter):
        for i in range(n_particles):
            score = objective_function(particles[i], K, T, τ, y0, t_sim)
            if score < personal_best_scores[i]:
                personal_best[i] = particles[i]
                personal_best_scores[i] = score
            if score < global_best_score:
                global_best = particles[i]
                global_best_score = score
        for i in range(n_particles):
            r1, r2 = np.random.rand(2)
            velocities[i] = (w * velocities[i] +
                             c1 * r1 * (personal_best[i] - particles[i]) +
                             c2 * r2 * (global_best - particles[i]))
            particles[i] += velocities[i]
            particles[i] = np.clip(particles[i], 
                                  [b[0] for b in bounds], 
                                  [b[1] for b in bounds])
    
    return global_best, global_best_score

# 9. Ziegler-Nichols PID整定
def zn_tuning(K, T, τ):
    Kp = 1.2 * T / (K * τ)
    Ti = 2 * τ
    Ki = Kp / Ti
    Td = 0.5 * τ
    Kd = Kp * Td
    
    return Kp, Ki, Kd

def main():
    # 加载数据
    data = load_data('temperature.csv')
    
    # 判断系统模型
    K, T, τ = system_identification(data)
    print(f"辨识结果: K={K:.4f}, T={T:.2f}s, τ={τ:.2f}s")
    print(f"传递函数: G(s) = {K:.4f}/({T:.2f}s + 1) * e^(-{τ:.2f}s)")
   
    Kp_zn, Ki_zn, Kd_zn = zn_tuning(K, T, τ)
    print(f"\nZiegler-Nichols PID参数: Kp={Kp_zn:.2f}, Ki={Ki_zn:.4f}, Kd={Kd_zn:.2f}")

    y0 = data['temperature'].iloc[0]
    t_sim = 4000  # 仿真时间(秒)
    dt = 0.5      # 时间步长
    
    # 运行ZN PID仿真
    t_zn, y_zn, u_zn = closed_loop_simulation(
        [Kp_zn, Ki_zn, Kd_zn], K, T, τ, y0, t_sim, dt
    )
    
    # 计算ZN PID性能指标
    perf_zn = calculate_performance(t_zn, y_zn, 35)
    print("\nZiegler-Nichols PID性能:")
    print(f"上升时间: {perf_zn['rise_time']:.2f}s")
    print(f"超调量: {perf_zn['overshoot']:.2f}%")
    print(f"调节时间: {perf_zn['settling_time']:.2f}s")
    print(f"ITAE: {perf_zn['ITAE']:.2f}")
    print(f"稳态误差: {perf_zn['steady_state_error']:.4f}")
    
    # PSO优化
    bounds = [(0, 50), (0, 5), (0, 200)]  
    best_params, best_score = pso_optimize(
        K, T, τ, y0, t_sim, bounds, n_particles=20, max_iter=50
    )
    print(f"\nPSO优化结果: Kp={best_params[0]:.2f}, Ki={best_params[1]:.4f}, Kd={best_params[2]:.2f}")
    print(f"最佳ITAE: {best_score:.2f}")
    
    # 运行优化后的PID仿真
    t_opt, y_opt, u_opt = closed_loop_simulation(
        best_params, K, T, τ, y0, t_sim, dt
    )
    
    # 计算优化PID性能指标
    perf_opt = calculate_performance(t_opt, y_opt, 35)
    print("\n优化PID性能:")
    print(f"上升时间: {perf_opt['rise_time']:.2f}s")
    print(f"超调量: {perf_opt['overshoot']:.2f}%")
    print(f"调节时间: {perf_opt['settling_time']:.2f}s")
    print(f"ITAE: {perf_opt['ITAE']:.2f}")
    print(f"稳态误差: {perf_opt['steady_state_error']:.4f}")
    
    # 绘图
    plt.figure(figsize=(12, 10))
    
    # 温度响应
    plt.subplot(2, 1, 1)
    plt.plot(t_zn, y_zn, 'b-', label='Ziegler-Nichols PID')
    plt.plot(t_opt, y_opt, 'r-', label='Optimized PID')
    plt.axhline(y=35, color='g', linestyle='--', label='Setpoint (35℃)')
    plt.title('Temperature Response')
    plt.xlabel('Time (s)')
    plt.ylabel('Temperature (℃)')
    plt.legend()
    plt.grid(True)
    plt.subplot(2, 1, 2)
    plt.plot(t_zn, u_zn, 'b-', label='Ziegler-Nichols PID')
    plt.plot(t_opt, u_opt, 'r-', label='Optimized PID')
    plt.title('Control Input')
    plt.xlabel('Time (s)')
    plt.ylabel('Voltage (V)')
    plt.legend()
    plt.grid(True)
    plt.tight_layout()
    plt.savefig('pid_control_performance.png')
    plt.show()
    
    results = {
        'system_params': {'K': K, 'T': T, 'τ': τ},
        'zn_params': {'Kp': Kp_zn, 'Ki': Ki_zn, 'Kd': Kd_zn},
        'zn_performance': perf_zn,
        'opt_params': {'Kp': best_params[0], 'Ki': best_params[1], 'Kd': best_params[2]},
        'opt_performance': perf_opt
    }
    
    return results

if __name__ == "__main__":
    results = main()